#include "stdafx.h"
#include "MyMath.h"

float saturate(float value)
{
	return min(1, max(0, value));
}

XMMATRIX mmath::constructBaseFromVector(XMVECTOR x)
{
	XMVECTOR y = XMVectorSwizzle(x, 1, 2, 0, 3);
	XMVECTOR z = XMVector3Cross(x, y);
	z = XMVector3Normalize(z);
	y = XMVector3Cross(z, x);

	XMMATRIX base;
	base.r[0] = x;
	base.r[1] = y;
	base.r[2] = z;

	return base;
}

XMVECTOR mmath::quaternionMultiply(FXMVECTOR Q1, FXMVECTOR Q2)
{
	static CONST XMVECTORF32 ControlWZYX = { 1.0f, 1.0f,-1.0f,-1.0f};
	static CONST XMVECTORF32 ControlZWXY = {-1.0f, 1.0f, 1.0f,-1.0f};
	static CONST XMVECTORF32 ControlYXWZ = { 1.0f,-1.0f, 1.0f,-1.0f};
	// Copy to SSE registers and use as few as possible for x86
	XMVECTOR Q2X = Q2;
	XMVECTOR Q2Y = Q2;
	XMVECTOR Q2Z = Q2;
	XMVECTOR vResult = Q2;
	// Splat with one instruction
	vResult = _mm_shuffle_ps(vResult,vResult,_MM_SHUFFLE(3,3,3,3));
	Q2X = _mm_shuffle_ps(Q2X,Q2X,_MM_SHUFFLE(0,0,0,0));
	Q2Y = _mm_shuffle_ps(Q2Y,Q2Y,_MM_SHUFFLE(1,1,1,1));
	Q2Z = _mm_shuffle_ps(Q2Z,Q2Z,_MM_SHUFFLE(2,2,2,2));
	// Retire Q1 and perform Q1*Q2W
	vResult = _mm_mul_ps(vResult,Q1);
	XMVECTOR Q1Shuffle = Q1;
	// Shuffle the copies of Q1
	Q1Shuffle = _mm_shuffle_ps(Q1Shuffle,Q1Shuffle,_MM_SHUFFLE(0,1,2,3));
	// Mul by Q1WZYX
	Q2X = _mm_mul_ps(Q2X,Q1Shuffle);
	Q1Shuffle = _mm_shuffle_ps(Q1Shuffle,Q1Shuffle,_MM_SHUFFLE(2,3,0,1));
	// Flip the signs on y and z
	Q2X = _mm_mul_ps(Q2X,ControlWZYX);
	// Mul by Q1ZWXY
	Q2Y = _mm_mul_ps(Q2Y,Q1Shuffle);
	Q1Shuffle = _mm_shuffle_ps(Q1Shuffle,Q1Shuffle,_MM_SHUFFLE(0,1,2,3));
	// Flip the signs on z and w
	Q2Y = _mm_mul_ps(Q2Y,ControlZWXY);
	// Mul by Q1YXWZ
	Q2Z = _mm_mul_ps(Q2Z,Q1Shuffle);
	vResult = _mm_add_ps(vResult,Q2X);
	// Flip the signs on x and w
	Q2Z = _mm_mul_ps(Q2Z,ControlYXWZ);
	Q2Y = _mm_add_ps(Q2Y,Q2Z);
	vResult = _mm_add_ps(vResult,Q2Y);
	return vResult;
}

XMVECTOR mmath::quaternionToEulerAngles( XMVECTOR quat )
{
	float eulerX, eulerY, eulerZ;
	float qX = XMVectorGetX(quat);
	float qY = XMVectorGetY(quat);
	float qZ = XMVectorGetZ(quat);
	float qW = XMVectorGetW(quat);

	float sqx = qX * qX;
	float sqy = qY * qY;
	float sqz = qZ * qZ;
	float sqw = qW * qW;

	float unit = sqx + sqy + sqz + sqw;
	float test = (qX * qW - qY * qZ);

	// Handle singularity
	if (test > 0.4999999f * unit)
	{
		eulerX = PI / 2;
		eulerY = 2.0f * atan2f(qY, qW);
		eulerZ = 0;
	}
	else if (test < -0.4999999f * unit)
	{
		eulerX = -PI / 2;
		eulerY = 2.0f * atan2f(qY, qW);
		eulerZ = 0;
	}
	else
	{
		float ey_Y = 2 * (qX * qZ + qY * qW);
		float ey_X = 1 - 2 * (sqy + sqx);
		float ez_Y = 2 * (qX * qY + qZ * qW);
		float ez_X = 1 - 2 * (sqx + sqz);
		eulerX = asinf(2 * test);
		eulerY = atan2f(ey_Y, ey_X);
		eulerZ = atan2f(ez_Y, ez_X);
	}

	return XMVectorSet(eulerX, eulerY, eulerZ, 0.0f);
}

XMMATRIX mmath::matrixFromAxes(XMVECTOR right, XMVECTOR up, XMVECTOR forward)
{
	return XMMATRIX(
		XMVectorGetX(right), XMVectorGetX(up), XMVectorGetX(forward), 0,
		XMVectorGetY(right), XMVectorGetY(up), XMVectorGetY(forward), 0,
		XMVectorGetZ(right), XMVectorGetZ(up), XMVectorGetZ(forward), 0,
		0, 0, 0, 1);
}
